#include <iostream>
#include <vector>
#include <string>
#include <alsa/asoundlib.h>
#include "utool_AudioRT/Config.h"
#include "utool_AudioRT/Audio_RT_Process_KF.h"
#include "utool_AudioRT/Audio_RT_Base_KF.h"
#include "utool_AudioRT/Audio_RT_FFT_KF.h"
#include "utool_AudioRT/SPE_BeamForming.h"
#include <thread>
#include <mutex>
#include <condition_variable>
#include <queue>
// 按键头文件
#include <thread>
#include "utool_AudioRT/HardwareThread_RT.h"
#include "utool_AudioRT/Globals.h"
// ASC声场分类RKNN头文件
#include "AscDeepFileRT/Preprocess_CMP_RT.h"
#include "AscDeepFileRT/DeepCMP_RT.h"
// 定义时间缓存队列,直接定义四路的队列用于存储麦克风数据
std::deque<std::deque<__fp16>> Time_Cache_LH( // 左前
    NUM_TIME,                     // 行数
    std::deque<__fp16>(WINDOW_SIZE, __fp16(0.0f)) // 每行初始化为包含 WINDOW_SIZE 个元素的 deque
);
std::deque<std::deque<__fp16>> Time_Cache_LB( // 左后
    NUM_TIME,                     // 行数
    std::deque<__fp16>(WINDOW_SIZE, __fp16(0.0f)) // 每行初始化为包含 WINDOW_SIZE 个元素的 deque
);
std::deque<std::deque<__fp16>> Time_Cache_RH( // 右前
    NUM_TIME,                     // 行数
    std::deque<__fp16>(WINDOW_SIZE, __fp16(0.0f)) // 每行初始化为包含 WINDOW_SIZE 个元素的 deque
);
std::deque<std::deque<__fp16>> Time_Cache_RB( // 右后
    NUM_TIME,                     // 行数
    std::deque<__fp16>(WINDOW_SIZE, __fp16(0.0f)) // 每行初始化为包含 WINDOW_SIZE 个元素的 deque
);

// 
std::vector<std::vector<__fp16>> Time_Cache_Matrix_LH; // 进行数组操作的原型数组--左前
std::vector<std::vector<__fp16>> Time_Cache_Matrix_LB; // 进行数组操作的原型数组--左后
std::vector<std::vector<__fp16>> Time_Cache_Matrix_RH; // 进行数组操作的原型数组--右前
std::vector<std::vector<__fp16>> Time_Cache_Matrix_RB; // 进行数组操作的原型数组--右后



std::vector<__fp16> Anly_Windows; // 分析窗口
std::vector<__fp16> Sys_Windows; // 分析窗口
std::vector<__fp16> result_AddSys; // 最后处理两帧叠加综合窗的结果
std::vector<__fp16> result_AddSys_R; // 最后处理两帧叠加综合窗的结果
std::vector<std::vector<__fp16>> mag_LH, phase_LH; // 幅度谱和相位谱结果--左前
std::vector<std::vector<__fp16>> mag_LB, phase_LB; // 幅度谱和相位谱结果--左后
std::vector<std::vector<__fp16>> mag_RH, phase_RH; // 幅度谱和相位谱结果--右前
std::vector<std::vector<__fp16>> mag_RB, phase_RB; // 幅度谱和相位谱结果--右后
// 创建处理器
FFTProcessor RTFFT_processor;
// 传统语音增强的参数
std::vector<std::vector<__fp16>> SPE_mag; // 语音增强后的幅度谱
std::vector<std::vector<__fp16>> SPE_pha; // 语音增强后的相位谱
std::vector<std::vector<__fp16>> SPE_mag_R; // 语音增强后的幅度谱--右边
std::vector<std::vector<__fp16>> SPE_pha_R; // 语音增强后的相位谱--右边


// 进行WDRC后的频谱
std::vector<std::vector<__fp16>> WDRC_mag;
// 进行FC后的频谱
std::vector<std::vector<__fp16>> FC_mag;
// WDRC的增益表格
std::vector<std::vector<__fp16>> WDRC_Table;
// FC的增益表格
std::vector<std::vector<__fp16>> FC_Table;
std::vector<__fp16> FC_CSRvalues;
std::vector<int> col_idx;
std::vector<int> row_ptr;
// 逆向变换
std::vector<std::vector<__fp16>> reconstructed_Time;
std::vector<std::vector<__fp16>> reconstructed_Time_R;

// 测试变量，把音频的输入输出存储下来煮酒
// 全局变量存储输入和输出的音频数据（16kHz * 20s = 320000 samples）
int time_num = 10;
std::vector<float> g_input_buffer_LH(time_num*SAMPLE_RATE, 0.0f);  // 输入缓存
std::vector<float> g_input_buffer_LB(time_num*SAMPLE_RATE, 0.0f);  // 输入缓存
std::vector<float> g_input_buffer_RH(time_num*SAMPLE_RATE, 0.0f);  // 输入缓存
std::vector<float> g_input_buffer_RB(time_num*SAMPLE_RATE, 0.0f);  // 输入缓存
std::vector<float> g_output_buffer(time_num*SAMPLE_RATE, 0.0f); // 输出缓存
unsigned int g_sample_counter = 0;                // 样本计数器

// 用于指定当前要进行音频塞入的行数
int input_num = 0; 
int hopsize_num = WINDOW_SIZE / HOP_SIZE; // 每一行的Hopsize数
int max_input_num = NUM_TIME + hopsize_num - 1;
int index_num = 0; //用于指定的行数
int move_up_flage = 0; //用于指定是否整体数组上移
int matrix_copy_flage = 0; //用于判断是否已经从队列中复制了数组数据


//-----------------------------------------
// 列出音频设备
//-----------------------------------------
void list_audio_devices()
{
    void **hints;
    if (snd_device_name_hint(-1, "pcm", &hints) < 0) {
        std::cerr << "Error: snd_device_name_hint()\n";
        return;
    }

    int index = 0;
    for (void **hint = hints; *hint != nullptr; hint++) {
        char *name = snd_device_name_get_hint(*hint, "NAME");
        char *desc = snd_device_name_get_hint(*hint, "DESC");

        if (name) {
            std::cout << index << ": " << name;
            if (desc)
                std::cout << " (" << desc << ")";
            std::cout << "\n";
            index++;
        }

        if (name) free(name);
        if (desc) free(desc);
    }

    snd_device_name_free_hint(hints);
}

// 打开 ALSA capture/playback
bool open_device(snd_pcm_t **handle, const char *devName, snd_pcm_stream_t stream, int channels)
{
    int err;
    if ((err = snd_pcm_open(handle, devName, stream, 0)) < 0) {
        std::cerr << "Cannot open device " << devName << ": " << snd_strerror(err) << "\n";
        return false;
    }

    snd_pcm_hw_params_t *hwParams;
    snd_pcm_hw_params_malloc(&hwParams);
    snd_pcm_hw_params_any(*handle, hwParams);
    snd_pcm_hw_params_set_access(*handle, hwParams, SND_PCM_ACCESS_RW_INTERLEAVED);
    snd_pcm_hw_params_set_format(*handle, hwParams, SND_PCM_FORMAT_FLOAT_LE);
    snd_pcm_hw_params_set_channels(*handle, hwParams, channels);
    unsigned int rate = SAMPLE_RATE;
    snd_pcm_hw_params_set_rate_near(*handle, hwParams, &rate, nullptr);

    snd_pcm_hw_params_set_period_size(*handle, hwParams, HOP_SIZE, 0);
    snd_pcm_hw_params_set_buffer_size(*handle, hwParams, HOP_SIZE * 2);

    snd_pcm_hw_params(*handle, hwParams);
    snd_pcm_hw_params_free(hwParams);
    snd_pcm_prepare(*handle);

    return true;
}


//-----------------------------------------
// FFT/IFFT 数据处理
//-----------------------------------------
// 多通道输入 -> 输出左右声道
void process_fft_multichannel(const std::vector<audio_sample_IN>& input,
                              std::vector<audio_sample_OUT>& output)
{
    // 调整索引
    input_num++;
    if(input_num > max_input_num){input_num = max_input_num;move_up_flage = 1;} // 此时已经塞入最后一帧了，缓存数组要上移
    index_num = std::max(input_num - hopsize_num, 0);

    if(move_up_flage == 1){ // 数组整体上移
        deque_move_up(Time_Cache_LH); 

        // 先注释一下
        deque_move_up(Time_Cache_LB); 
        deque_move_up(Time_Cache_RH); 
        deque_move_up(Time_Cache_RB); 
    }
    if(index_num >= 1){ // 已经可以复制了
        deque_copy_start_tar(Time_Cache_LH, index_num - 1, index_num);

        // 先注释一下
        deque_copy_start_tar(Time_Cache_LB, index_num - 1, index_num);
        deque_copy_start_tar(Time_Cache_RH, index_num - 1, index_num);
        deque_copy_start_tar(Time_Cache_RB, index_num - 1, index_num);
    }// 完成当前指定帧的复制

    if (Is_RKNN_Flag.load() == 0){
        size_t pos = current_write_pos.load();
        size_t remaining_space = Input_RKNN.size() - pos;
        size_t frames_to_write = std::min(static_cast<size_t>(HOP_SIZE), remaining_space);
        // 只提取left_H声道数据
        for (size_t i = 0; i < frames_to_write; i++) {
            Input_RKNN[pos + i] = input[i].left_H;
        }
        pos += frames_to_write;
        current_write_pos.store(pos);
        // 检查缓冲区是否已满
        if (pos >= Input_RKNN.size()) {
            Is_RKNN_Flag.store(1);
        }
    }


    // 将样本数据进行push
    for (unsigned int i = 0; i < HOP_SIZE; i++) {
        if(index_num == 0){
            // 左前
            //删除开头的点  
            Time_Cache_LH[index_num].pop_front();
            //新的样本点推入末尾
            Time_Cache_LH[index_num].push_back(static_cast<__fp16>(input[i].left_H));
            // 先注释一下
            // 左后
            //删除开头的点  
            Time_Cache_LB[index_num].pop_front();
            //新的样本点推入末尾
            Time_Cache_LB[index_num].push_back(static_cast<__fp16>(input[i].left_B));
            // 右前
            //删除开头的点  
            Time_Cache_RH[index_num].pop_front();
            //新的样本点推入末尾
            Time_Cache_RH[index_num].push_back(static_cast<__fp16>(input[i].right_H));
            // 右后
            //删除开头的点  
            Time_Cache_RB[index_num].pop_front();
            //新的样本点推入末尾
            Time_Cache_RB[index_num].push_back(static_cast<__fp16>(input[i].right_B));
        }
        else{
            Time_Cache_LH[index_num][HOP_SIZE*(hopsize_num - 1) + i] = input[i].left_H;

            // 先注释一下
            Time_Cache_LB[index_num][HOP_SIZE*(hopsize_num - 1) + i] = input[i].left_B;
            Time_Cache_RH[index_num][HOP_SIZE*(hopsize_num - 1) + i] = input[i].right_H;
            Time_Cache_RB[index_num][HOP_SIZE*(hopsize_num - 1) + i] = input[i].right_B;
        }
    }
    // 表示缓存数据已经满了，可以进行输出了
    if(input_num == max_input_num){
        // 在这里面进行音频的算法流处理
        // 1.先复制上面的二维队列到数组中以方便操作
        if(matrix_copy_flage == 0){ // 还没复制队列数据，要开始复制队列数据
            // 把四路麦克风队列变数组
            Time_Cache_Matrix_LH = deque_to_matrix(Time_Cache_LH);

            // 先注释一下
            Time_Cache_Matrix_LB = deque_to_matrix(Time_Cache_LB);
            Time_Cache_Matrix_RH = deque_to_matrix(Time_Cache_RH);
            Time_Cache_Matrix_RB = deque_to_matrix(Time_Cache_RB);

            //变数组完成
            matrix_copy_flage = 1;
            // 添加窗函数，每一帧都添加分析窗函数
            Time_Cache_Matrix_LH = apply_window_multiply(Time_Cache_Matrix_LH,Anly_Windows);

            // 先注释一下
            Time_Cache_Matrix_LB = apply_window_multiply(Time_Cache_Matrix_LB,Anly_Windows);
            Time_Cache_Matrix_RH = apply_window_multiply(Time_Cache_Matrix_RH,Anly_Windows);
            Time_Cache_Matrix_RB = apply_window_multiply(Time_Cache_Matrix_RB,Anly_Windows);

            // 把添加了窗函数的时域信号直接进行FFT，然后返回幅度谱和相位谱
            RTFFT_processor.compute_spectrum(Time_Cache_Matrix_LH, mag_LH, phase_LH); 

            // 先注释一下
            RTFFT_processor.compute_spectrum(Time_Cache_Matrix_LB, mag_LB, phase_LB); 
            RTFFT_processor.compute_spectrum(Time_Cache_Matrix_RH, mag_RH, phase_RH); 
            RTFFT_processor.compute_spectrum(Time_Cache_Matrix_RB, mag_RB, phase_RB); 
            // // 进行WDRC
            // RT_WDRC(mag,WDRC_mag);
            RTFFT_processor.reconstruct_signal(mag_LH, phase_LH, reconstructed_Time);
            RTFFT_processor.reconstruct_signal(mag_RH, phase_RH, reconstructed_Time_R);
        }
        else{ 
            // 左前
            shift_matrix_up(Time_Cache_Matrix_LH,Time_Cache_LH); // 把数组整体上移一位，并且把队列的最后一行数据放到数组最后一行中
            // 添加分析窗函数(只对最后一行)
            multiply_last_n_rows_inplace(Time_Cache_Matrix_LH,Anly_Windows,1);
            // mag和phase向上移动一行，并且把加窗了的时域进行FFT后分别补充到mag和phase
            shift_matrix_MAG_PHASE_up(Time_Cache_Matrix_LH, mag_LH, phase_LH, RTFFT_processor);

            // 先注释一下
            // 左后
            shift_matrix_up(Time_Cache_Matrix_LB,Time_Cache_LB); // 把数组整体上移一位，并且把队列的最后一行数据放到数组最后一行中
            // 添加分析窗函数(只对最后一行)
            multiply_last_n_rows_inplace(Time_Cache_Matrix_LB,Anly_Windows,1);
            // mag和phase向上移动一行，并且把加窗了的时域进行FFT后分别补充到mag和phase
            shift_matrix_MAG_PHASE_up(Time_Cache_Matrix_LB, mag_LB, phase_LB, RTFFT_processor);
            // 右前
            shift_matrix_up(Time_Cache_Matrix_RH,Time_Cache_RH); // 把数组整体上移一位，并且把队列的最后一行数据放到数组最后一行中
            // 添加分析窗函数(只对最后一行)
            multiply_last_n_rows_inplace(Time_Cache_Matrix_RH,Anly_Windows,1);
            // mag和phase向上移动一行，并且把加窗了的时域进行FFT后分别补充到mag和phase
            shift_matrix_MAG_PHASE_up(Time_Cache_Matrix_RH, mag_RH, phase_RH, RTFFT_processor);
            // 右后
            shift_matrix_up(Time_Cache_Matrix_RB,Time_Cache_RB); // 把数组整体上移一位，并且把队列的最后一行数据放到数组最后一行中
            // 添加分析窗函数(只对最后一行)
            multiply_last_n_rows_inplace(Time_Cache_Matrix_RB,Anly_Windows,1);
            // mag和phase向上移动一行，并且把加窗了的时域进行FFT后分别补充到mag和phase
            shift_matrix_MAG_PHASE_up(Time_Cache_Matrix_RB, mag_RB, phase_RB, RTFFT_processor);


            if(Env_Class_Global.load() == 2 || Env_Class_Global.load() == 3){ // 当前是含噪环境才降噪
                // std::cout << "开启方向性麦克风降噪" << std::endl;
                // 进行麦克风阵列的输出
                DeNoise_MutileSpeech(mag_LH,phase_LH,mag_LB,phase_LB,SPE_mag,SPE_pha);
                DeNoise_MutileSpeech(mag_RH,phase_RH,mag_RB,phase_RB,SPE_mag_R,SPE_pha_R);
                // 把mag和phase结合，只对最后两行进行IFFT
                RTFFT_processor.reconstruct_signal_Only(SPE_mag, SPE_pha, reconstructed_Time);
                RTFFT_processor.reconstruct_signal_Only(SPE_mag_R, SPE_pha_R, reconstructed_Time_R);
            }
            else{
                RTFFT_processor.reconstruct_signal_Only(mag_LH, phase_LH, reconstructed_Time);
                RTFFT_processor.reconstruct_signal_Only(mag_RH, phase_RH, reconstructed_Time_R);
            }

            // RTFFT_processor.reconstruct_signal_Only(mag_LH, phase_LH, reconstructed_Time);
            // RTFFT_processor.reconstruct_signal_Only(mag_RH, phase_RH, reconstructed_Time_R);
        }
        // 把幅度谱送入模型中看看效果

        // 最后两行乘上综合窗并进行叠加还原
        result_AddSys = Generate_SysResult(reconstructed_Time,Sys_Windows,2);
        result_AddSys_R = Generate_SysResult(reconstructed_Time_R,Sys_Windows,2);
        for (unsigned int i = 0; i < HOP_SIZE; ++i) {
            //取出开头样本点输出到左右声道
            output[i].left = result_AddSys[i]*10;
            output[i].right = result_AddSys_R[i]*10;
        }
    }
}


// 线程方式处理


std::queue<std::vector<audio_sample_IN>> audioQueue;
std::mutex mtx;
std::condition_variable cv;
bool running = true;

// PCM 读取线程
void capture_thread(snd_pcm_t *handle) {
    std::vector<float> buf(HOP_SIZE * IN_NUM_CHANNELS);
    while (running) {
        int err = snd_pcm_readi(handle, buf.data(), HOP_SIZE);
        if (err < 0) snd_pcm_prepare(handle);

        // 转换为 audio_sample_IN 结构体
        std::vector<audio_sample_IN> frame(HOP_SIZE);
        for (int i = 0; i < HOP_SIZE; i++) {
            frame[i].left_H  = buf[i*IN_NUM_CHANNELS + 0];
            frame[i].left_B  = buf[i*IN_NUM_CHANNELS + 1];
            frame[i].right_H = buf[i*IN_NUM_CHANNELS + 2];
            frame[i].right_B = buf[i*IN_NUM_CHANNELS + 3];
        }
        {
            std::lock_guard<std::mutex> lock(mtx);
            // 方法1：丢弃最旧帧
            while (audioQueue.size() >= MAX_QUEUE_FRAMES) {
                audioQueue.pop(); // 弹出最旧帧
            }
            audioQueue.push(frame);
        }
        cv.notify_one();
    }
}

// DSP处理 + PCM写入线程
void process_thread(snd_pcm_t *handle) {
    std::vector<audio_sample_OUT> outFrame(HOP_SIZE);  // 输出结构体数组
    std::vector<float> outBuf(HOP_SIZE * OUT_NUM_CHANNELS); // PCM interleaved buffer

    while (running) {
        std::vector<audio_sample_IN> inFrame;
        {
            std::unique_lock<std::mutex> lock(mtx);
            cv.wait(lock, [](){ return !audioQueue.empty(); });
            inFrame = audioQueue.front();
            audioQueue.pop();
        }

        // 调用处理函数
        process_fft_multichannel(inFrame, outFrame);

        // 转 interleaved float 写入 ALSA
        for (int i = 0; i < HOP_SIZE; i++) {
            outBuf[i*OUT_NUM_CHANNELS + 0] = outFrame[i].left;
            outBuf[i*OUT_NUM_CHANNELS + 1] = outFrame[i].right;
        }

        int err = snd_pcm_writei(handle, outBuf.data(), HOP_SIZE);
        if (err < 0) snd_pcm_prepare(handle);
    }
}




//-----------------------------------------
// 主程序
//-----------------------------------------
int main()
{
    // 初始化ASC的RKNN模型
    // 加载模型
    deepcmp.LoadModel("/home/orangepi/HearingAID_Model/weights/ASC_CPM.rknn");
    mel_filter_bank = create_mel_filter_bank(SAMPLE_RATE, WINDOW_SIZE_ASC, NUM_MELS);
    // 生成分析窗(输入FFT的时域加窗)
    Anly_Windows = asymmetric_Analy_windows(HOP_SIZE,WINDOW_SIZE,10);
    // 生成综合窗(IFFT输出的时域结果加窗)
    Sys_Windows = asymmetric_Sys_windows(HOP_SIZE,WINDOW_SIZE,10);
    // 读取WDRC表格
    Read_WDRC_Table(WDRC_Table);
    // 读取FC表格
    Read_FC_Table(FC_Table);
    convertToCSR(FC_Table,FC_CSRvalues,col_idx,row_ptr);

    // // 按键的IO初始化
    // Key_Gpio_Init();
    // // 启动 GPIO 监控线程
    // std::thread gpioThread(gpioMonitorThread);
    
    // 启动ASC的RKNN线程
    std::thread rknn_thread(AscRknnThread); // 先实现，后面再优化吧



    std::cout << "Available ALSA PCM devices:\n";
    list_audio_devices();

    int inIndex, outIndex;
    std::cout << "Enter input device index: ";
    std::cin >> inIndex;
    std::cout << "Enter output device index: ";
    std::cin >> outIndex;

    // 再次获取设备名
    void **hints;
    snd_device_name_hint(-1, "pcm", &hints);
    std::vector<std::string> devices;
    for (void **hint = hints; *hint != nullptr; hint++) {
        char *name = snd_device_name_get_hint(*hint, "NAME");
        if (name)
            devices.push_back(name);
        if (name) free(name);
    }
    snd_device_name_free_hint(hints);

    if (inIndex < 0 || inIndex >= (int)devices.size()) {
        std::cerr << "Invalid input device index\n";
        return -1;
    }
    if (outIndex < 0 || outIndex >= (int)devices.size()) {
        std::cerr << "Invalid output device index\n";
        return -1;
    }

    std::string inDev  = devices[inIndex];
    std::string outDev = devices[outIndex];

    std::cout << "Using input device:  " << inDev  << "\n";
    std::cout << "Using output device: " << outDev << "\n";

    snd_pcm_t *captureHandle = nullptr;
    snd_pcm_t *playbackHandle = nullptr;

    if (!open_device(&captureHandle, inDev.c_str(), SND_PCM_STREAM_CAPTURE, IN_NUM_CHANNELS)) return -1;
    if (!open_device(&playbackHandle, outDev.c_str(), SND_PCM_STREAM_PLAYBACK, OUT_NUM_CHANNELS)) return -1;



    std::cout << "Real-time streaming started...\n";

    std::thread t1(capture_thread, captureHandle);
    std::thread t2(process_thread, playbackHandle);

    t1.join();
    t2.join();

    // snd_pcm_close(captureHandle);
    // snd_pcm_close(playbackHandle);

    return 0;
}
